#include "Frame.h"
#include <boost\thread\thread.hpp>
#include "LabelEquivalenceSeg.h"

boost::shared_mutex _lock;

Frame::Frame(int s) :
surfaces(), surfaces_global(), dn(s), dv(s), size(s){
	segmentMap = new bool[size];
	host_label = new int[size];
	host_vertice = new float3[size];
}

Frame::~Frame(){
	delete[] segmentMap;
	delete[] host_label;
	delete[] host_vertice;
	surfaces.~SurfacesHandler();
	surfaces_global.~SurfacesHandler();
	dn.~DeviceNormal();
	dv.~DeviceVertice();
}


void Frame::setCameraPos(cameraPose* pos){
	this->pos = *pos;
}

void Frame::setVertices(float3* in){
	boost::upgrade_lock<boost::shared_mutex> lock(_lock);
	dv.insertDataFromHost(in);
	lock.unlock();
}

void Frame::setNormal(float3* in){
	boost::upgrade_lock<boost::shared_mutex> lock(_lock);
	dn.insertDataFromHost(in);
	lock.unlock();
}

void Frame::segmentPlanarStructures(LabelEquivalenceSeg* seg){
	//clear residuals
	surfaces.reset();

	//Extract normal vectors, 3D vertices from KinFu
	boost::shared_lock<boost::shared_mutex> lock(_lock);
	dn.segmentNormalMap();
	dv.extractDataToHost(host_vertice);
	segmentMap = dn.getSegmentationResult();
	lock.unlock();

	//Segment planar structures 
	seg->labelImage(segmentMap);
	seg->viewSegmentResult();

	//Process detected planar structures
	surfaces.processSurfaces(seg, host_vertice);
	
}

void Frame::integratePlanarStructures(){
	surfaces_global.mergeSurfaces(&surfaces);
}